/* Copyright 2017 The Apollo Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/

#include "modules/map/hdmap/adapter/odr_parser/objects_odr_parser.h"

#include <string>
#include <vector>

#include "modules/map/hdmap/adapter/xml_parser/util_xml_parser.h"

namespace apollo {
namespace hdmap {
namespace adapter {

struct ObjectAttr {
    std::string name;
    int id;
    double s;
    double t;
    double zOffset;
    double validLength;
    std::string orientation;
    double length;
    double width;
    double radius;
    double height;
    double hdg;
    double pitch;
    double roll;
};

Status ObjectsOdrParser::ParseObjects(const tinyxml2::XMLElement& xml_node,
                               Road& road_internal)
{

  auto objects_node = xml_node.FirstChildElement("objects");
  if (objects_node) {
    auto object_node = objects_node->FirstChildElement("object");
    while (object_node) {
      RoadObject object;
      std::string object_type;
      int checker =
          UtilXmlParser::QueryStringAttribute(*object_node, "type", &object_type);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse object type.";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      std::string upper_str = UtilXmlParser::ToUpper(object_type);
      if (upper_str == "STOP LINE") {
        object.type = ROAD_STOPLINE;
      } else if (upper_str == "STANDARD CROSSWALK" || upper_str == "CROSSWALK" ) {
        object.type = ROAD_CROSSWALK;
      } else {
        AINFO << "Warning: not supported object type!";
        object_node = object_node->NextSiblingElement("object");
        continue;
      }

      RETURN_IF_ERROR(ParseObject(*object_node, object));

      road_internal.objects.push_back(object);
      object_node = object_node->NextSiblingElement("object");
    }
  }

  return Status::OK();
}

Status ObjectsOdrParser::ParseObject(const tinyxml2::XMLElement& xml_node,
                                       RoadObject& object)
{
  
  int checker;
  checker = UtilXmlParser::QueryStringAttribute(xml_node, "name", &object.name);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse object type attribute";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  // id
  checker = UtilXmlParser::QueryStringAttribute(xml_node, "id", &object.id);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse object id attribute";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  checker = xml_node.QueryDoubleAttribute("s", &object.s);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse object s attribute";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  checker = xml_node.QueryDoubleAttribute("t", &object.t);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse object t attribute";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  checker = xml_node.QueryDoubleAttribute("zOffset", &object.zOffset);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse object zOffset attribute";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  checker = xml_node.QueryDoubleAttribute("validLength", &object.validLength);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse object validLength attribute";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  checker = UtilXmlParser::QueryStringAttribute(xml_node, "orientation", &object.orientation);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse object orientation attribute";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  checker = xml_node.QueryDoubleAttribute("length", &object.length);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse object length attribute";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  checker = xml_node.QueryDoubleAttribute("width", &object.width);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse object width attribute";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  checker = xml_node.QueryDoubleAttribute("radius", &object.radius);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse object radius attribute";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  checker = xml_node.QueryDoubleAttribute("height", &object.height);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse object height attribute";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  checker = xml_node.QueryDoubleAttribute("hdg", &object.hdg);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse object hdg attribute";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  checker = xml_node.QueryDoubleAttribute("pitch", &object.pitch);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse object pitch attribute";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  checker = xml_node.QueryDoubleAttribute("roll", &object.roll);
  if (checker != tinyxml2::XML_SUCCESS) {
    std::string err_msg = "Error parse object roll attribute";
    return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
  }

  auto outline_node = xml_node.FirstChildElement("outline");
  if (outline_node) {
    ObjectOutline outline;

    auto cornerLocal_node = outline_node->FirstChildElement("cornerLocal");
    while (cornerLocal_node) {
      CornerLocal cornerLocal;
      checker = cornerLocal_node->QueryDoubleAttribute("u", &cornerLocal.u);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse cornerLocal u attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      checker = cornerLocal_node->QueryDoubleAttribute("v", &cornerLocal.v);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse cornerLocal v attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      checker = cornerLocal_node->QueryDoubleAttribute("z", &cornerLocal.z);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse cornerLocal z attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }

      checker = cornerLocal_node->QueryDoubleAttribute("height", &cornerLocal.height);
      if (checker != tinyxml2::XML_SUCCESS) {
        std::string err_msg = "Error parse cornerLocal height attribute";
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }
      
      outline.cornerLocals.push_back(cornerLocal);
      cornerLocal_node = cornerLocal_node->NextSiblingElement("cornerLocal");
    }

    auto cornerRoad_node = outline_node->FirstChildElement("cornerRoad");
    while (cornerRoad_node) {
      printf("TODO: support corner road node!!!\n");
      break;
    }
    if (object.type == ROAD_CROSSWALK)
    {
      if (outline.cornerLocals.size() < 4)
      {
        std::string err_msg = "Error parse outlines size wrong : object id " + object.id;
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }
    }
    else if (object.type == ROAD_STOPLINE)
    {
      if (outline.cornerLocals.size() < 2)
      {
        std::string err_msg = "Error parse outlines size wrong : object id " + object.id;
        return Status(apollo::common::ErrorCode::HDMAP_DATA_ERROR, err_msg);
      }
    }
    object.outlines.push_back(outline);

    outline_node = outline_node->NextSiblingElement("outline");
  }
  

  return Status::OK();
}                                      

}  // namespace adapter
}  // namespace hdmap
}  // namespace apollo
